#include "RigidBodyCollisionHandler.h"

namespace coolpool
{
	RigidBodyCollisionHandler::RigidBodyCollisionHandler()
	{
	}

	RigidBodyCollisionHandler::RigidBodyCollisionHandler(const RigidBodyCollisionHandler & ball_collision_handler) :
		CollisionHandler(ball_collision_handler)
	{
	}

	CollisionHandler * RigidBodyCollisionHandler::clone() const
	{
		return new RigidBodyCollisionHandler(*this);
	}

	RigidBodyCollisionHandler::~RigidBodyCollisionHandler()
	{
	}

	void RigidBodyCollisionHandler::handle(const Collision * collision) throw (UnexpectedInputException)
	{
		assert(dynamic_cast<RigidBody *>(collision->first));
		assert(dynamic_cast<RigidBody *>(collision->second));

		if (collision->second->isStatic())
			handleCollisionWithStatic(collision);
		else
			handleCollisionWithDynamic(collision);
	}

	void RigidBodyCollisionHandler::handleCollisionWithDynamic(const Collision * collision)
	{
		RigidBody * rigid_body1 = static_cast<RigidBody *> (collision->first);
		RigidBody * rigid_body2 = static_cast<RigidBody *> (collision->second);
		int type_id = rigid_body2->getType();
		Vector3D collision_point = collision->collision_point;
		Vector3D collision_normal = collision->collision_normal;

		Vector3D col_point_vel1 = rigid_body1->pointVelocity(collision_point);
		Vector3D col_point_vel2 = rigid_body2->pointVelocity(collision_point);
		// Compute relative velocity.
		Vector3D col_point_rel_vel = col_point_vel1 - col_point_vel2;
		double vel_rel = collision_normal.dot(col_point_rel_vel);

		// Decompose the velocity into two parts, normal and tangential.
		Vector3D normal_part = collision_normal * vel_rel;
		Vector3D tangential_part = col_point_rel_vel - normal_part;

		// Vector from the center of the object to the collision point.
		Vector3D r1 = collision_point - rigid_body1->getCenterOfMassLocation();
		Vector3D r2 = collision_point - rigid_body2->getCenterOfMassLocation();

		// Calculating the impulse size
		double numerator = -(1 + rigid_body1->getCoefficientOfRestitionWith(type_id)) * vel_rel;
		Vector3D r1_cross_col_normal = r1 * collision_normal;
		Vector3D r2_cross_col_normal = r2 * collision_normal;
		matrix_tools::transform(rigid_body1->getInertiaTensorInverse(), r1_cross_col_normal, r1_cross_col_normal);
		matrix_tools::transform(rigid_body2->getInertiaTensorInverse(), r2_cross_col_normal, r2_cross_col_normal);
		double term1 = 1 / rigid_body1->getMass();
		double term2 = 1 / rigid_body2->getMass();
		double term3 = collision_normal.dot(r1_cross_col_normal * r1);
		double term4 = collision_normal.dot(r2_cross_col_normal * r2);

		double normal_impulse_size = (numerator / (term1 + term2 + term3 + term4));
		Vector3D impulse = (collision_normal * normal_impulse_size);

		// Set new velocities.
		rigid_body1->setLinearVelocity(rigid_body1->getLinearVelocity() + impulse * (1 / rigid_body1->getMass()));
		rigid_body2->setLinearVelocity(rigid_body2->getLinearVelocity() - impulse * (1 / rigid_body2->getMass()));
		rigid_body1->setAngularMomentum(rigid_body1->getAngularMomentum() + r1 * impulse);
		rigid_body2->setAngularMomentum(rigid_body2->getAngularMomentum() - r2 * impulse);
		addFrictionToImpulse(collision, rigid_body1, rigid_body2, normal_impulse_size);
	}

	void RigidBodyCollisionHandler::handleCollisionWithStatic(const Collision * collision)
	{
		RigidBody * rigid_body1 = static_cast<RigidBody *> (collision->first);
		RigidBody * rigid_body2 = static_cast<RigidBody *> (collision->second);
		int type_id = rigid_body2->getType();
		Vector3D collision_point = collision->collision_point;
		Vector3D collision_normal = collision->collision_normal;

		Vector3D col_point_rel_vel = rigid_body1->pointVelocity(collision_point);

		// Relative velocity agains a static object is the velocity of the object itself.
		double vel_rel = collision_normal.dot(col_point_rel_vel);

		// Decompose the velocity into two parts, normal and tangential.
		Vector3D normal_part = collision_normal * vel_rel;
		Vector3D tangential_part = col_point_rel_vel - normal_part;

		// Vector from the center of the object to the collision point
		Vector3D r1 = collision_point - rigid_body1->getCenterOfMassLocation();

		// Calculating the impulse size
		Vector3D ra_cross_col_normal = r1 * collision_normal;
		matrix_tools::transform(rigid_body1->getInertiaTensorInverse(), ra_cross_col_normal, ra_cross_col_normal);
		double numerator = -(1 + rigid_body1->getCoefficientOfRestitionWith(type_id)) * vel_rel;
		double term1 = 1 / rigid_body1->getMass();
		double term2 = collision_normal.dot(ra_cross_col_normal * r1);

		double normal_impulse_size = (numerator / (term1 + term2));
		Vector3D impulse = (collision_normal * normal_impulse_size);

		// Set new velocities.
		rigid_body1->setLinearVelocity(rigid_body1->getLinearVelocity() + impulse * (1 / rigid_body1->getMass()));
		rigid_body1->setAngularMomentum(rigid_body1->getAngularMomentum() + r1 * impulse);

		addFrictionToImpulse(collision, rigid_body1, rigid_body2, normal_impulse_size);
	}
}

